#include "debug_manager/debug_lidar_node.h"

// ros2 run debug_manager debug_lidar_node --ros-args -p scan_topic:=/scan

DebugLidarNode::DebugLidarNode(): Node("debug_lidar_node"){
    this->declare_parameter<std::string>("scan_topic", "/scan");
    this->get_parameter("scan_topic", scan_topic_);
    RCLCPP_INFO(this->get_logger(), "scan_topic: %s", scan_topic_.c_str());
    lidar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
        scan_topic_, 10, std::bind(&DebugLidarNode::lidarCallback, this, std::placeholders::_1));
}

DebugLidarNode::~DebugLidarNode(){

}


// 激光雷达订阅函数
void DebugLidarNode::lidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(), "---------------------------------");
    RCLCPP_INFO(this->get_logger(), "Received scan. sec: %d, frame_id: %s", 
        msg->header.stamp.sec, msg->header.frame_id.c_str());
    RCLCPP_INFO(this->get_logger(), "angle_min: %0.3f, angle_max: %0.3f, angle_increment: %0.3f", 
        msg->angle_min, msg->angle_max, msg->angle_increment);
    RCLCPP_INFO(this->get_logger(), "range_min: %0.3f, range_max: %0.3f, ranges_size: %ld, intensities_size: %ld",
        msg->range_min, msg->range_max, msg->ranges.size(), msg->intensities.size());
    RCLCPP_INFO(this->get_logger(), "time_increment: %0.6f, scan_time: %0.4f", 
        msg->time_increment, msg->scan_time);
}


int main(int argc, char * argv[]){
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DebugLidarNode>());
    rclcpp::shutdown();
    return 0;
  }